/**
 * @brief The mat4base class - OpenGL compatible matrix.
 * This matrix is compatible with OpenGL, so you can directly load and save it
 * using glLoadMatrix[d|f]() and glGetDoublev(GL_MODELVIEW_MATRIX, myptr)
 */

#pragma once

#include "vec4.h"
#include "mat3.h"
#include <string.h>
#include <math.h>

template <class treal>
class mat4base
{
public:
    mat4base(void)
    {
        loadIdentity();
    }

    explicit mat4base(treal const * src)
    {
        memcpy(data, src, sizeof(data));
    }

    mat4base(
        treal const& v00, treal const& v10, treal const& v20, treal const& v30,
        treal const& v01, treal const& v11, treal const& v21, treal const& v31,
        treal const& v02, treal const& v12, treal const& v22, treal const& v32,
        treal const& v03, treal const& v13, treal const& v23, treal const& v33
        )
    {
        a00 = v00; a10 = v10; a20 = v20; a30 = v30;
        a01 = v01; a11 = v11; a21 = v21; a31 = v31;
        a02 = v02; a12 = v12; a22 = v22; a32 = v32;
        a03 = v03; a13 = v13; a23 = v23; a33 = v33;
    }

    inline operator const treal*()const
    {
        return &data[0];
    }

    inline operator treal*()
    {
        return &data[0];
    }

    inline mat4base& operator*=(mat4base const& rhs)
    {
        treal result[4][4];

        result[0][0] = a00*rhs.a00 + a01*rhs.a10 + a02*rhs.a20 + a03*rhs.a30;
        result[0][1] = a10*rhs.a00 + a11*rhs.a10 + a12*rhs.a20 + a13*rhs.a30;
        result[0][2] = a20*rhs.a00 + a21*rhs.a10 + a22*rhs.a20 + a23*rhs.a30;
        result[0][3] = a30*rhs.a00 + a31*rhs.a10 + a32*rhs.a20 + a33*rhs.a30;

        result[1][0] = a00*rhs.a01 + a01*rhs.a11 + a02*rhs.a21 + a03*rhs.a31;
        result[1][1] = a10*rhs.a01 + a11*rhs.a11 + a12*rhs.a21 + a13*rhs.a31;
        result[1][2] = a20*rhs.a01 + a21*rhs.a11 + a22*rhs.a21 + a23*rhs.a31;
        result[1][3] = a30*rhs.a01 + a31*rhs.a11 + a32*rhs.a21 + a33*rhs.a31;

        result[2][0] = a00*rhs.a02 + a01*rhs.a12 + a02*rhs.a22 + a03*rhs.a32;
        result[2][1] = a10*rhs.a02 + a11*rhs.a12 + a12*rhs.a22 + a13*rhs.a32;
        result[2][2] = a20*rhs.a02 + a21*rhs.a12 + a22*rhs.a22 + a23*rhs.a32;
        result[2][3] = a30*rhs.a02 + a31*rhs.a12 + a32*rhs.a22 + a33*rhs.a32;

        result[3][0] = a00*rhs.a03 + a01*rhs.a13 + a02*rhs.a23 + a03*rhs.a33;
        result[3][1] = a10*rhs.a03 + a11*rhs.a13 + a12*rhs.a23 + a13*rhs.a33;
        result[3][2] = a20*rhs.a03 + a21*rhs.a13 + a22*rhs.a23 + a23*rhs.a33;
        result[3][3] = a30*rhs.a03 + a31*rhs.a13 + a32*rhs.a23 + a33*rhs.a33;
        memcpy(data, result, sizeof(data));

        return *this;
    }

    inline mat4base const operator*(mat4base const& rhs)const
    {
        treal result[4][4];

        result[0][0] = a00*rhs.a00 + a01*rhs.a10 + a02*rhs.a20 + a03*rhs.a30;
        result[0][1] = a10*rhs.a00 + a11*rhs.a10 + a12*rhs.a20 + a13*rhs.a30;
        result[0][2] = a20*rhs.a00 + a21*rhs.a10 + a22*rhs.a20 + a23*rhs.a30;
        result[0][3] = a30*rhs.a00 + a31*rhs.a10 + a32*rhs.a20 + a33*rhs.a30;

        result[1][0] = a00*rhs.a01 + a01*rhs.a11 + a02*rhs.a21 + a03*rhs.a31;
        result[1][1] = a10*rhs.a01 + a11*rhs.a11 + a12*rhs.a21 + a13*rhs.a31;
        result[1][2] = a20*rhs.a01 + a21*rhs.a11 + a22*rhs.a21 + a23*rhs.a31;
        result[1][3] = a30*rhs.a01 + a31*rhs.a11 + a32*rhs.a21 + a33*rhs.a31;

        result[2][0] = a00*rhs.a02 + a01*rhs.a12 + a02*rhs.a22 + a03*rhs.a32;
        result[2][1] = a10*rhs.a02 + a11*rhs.a12 + a12*rhs.a22 + a13*rhs.a32;
        result[2][2] = a20*rhs.a02 + a21*rhs.a12 + a22*rhs.a22 + a23*rhs.a32;
        result[2][3] = a30*rhs.a02 + a31*rhs.a12 + a32*rhs.a22 + a33*rhs.a32;

        result[3][0] = a00*rhs.a03 + a01*rhs.a13 + a02*rhs.a23 + a03*rhs.a33;
        result[3][1] = a10*rhs.a03 + a11*rhs.a13 + a12*rhs.a23 + a13*rhs.a33;
        result[3][2] = a20*rhs.a03 + a21*rhs.a13 + a22*rhs.a23 + a23*rhs.a33;
        result[3][3] = a30*rhs.a03 + a31*rhs.a13 + a32*rhs.a23 + a33*rhs.a33;

        return mat4base(&result[0][0]);
    }

    inline basevec4<treal> operator*(basevec4<treal> const& vec) const
    {
        treal x = dot(getRow(0), vec);
        treal y = dot(getRow(1), vec);
        treal z = dot(getRow(2), vec);
        treal w = dot(getRow(3), vec);

        return basevec4<treal>(x, y, z, w);
    }

    inline basevec3<treal> transform(const basevec3<treal> &point) const
    {
        return ((*this) * basevec4<treal>(point, 1.0)).project();
    }

    inline void loadTranslation(treal dx, treal dy, treal dz)
    {
        loadIdentity();
        x = dx;
        y = dy;
        z = dz;
    }

    inline void translate(treal dx, treal dy, treal dz)
    {
        mat4base translationMatrix;
        translationMatrix.x = dx;
        translationMatrix.y = dy;
        translationMatrix.z = dz;

        *this *= translationMatrix;
    }

    inline void loadScale(treal sx0, treal sy0, treal sz0)
    {
        loadIdentity();
        sx = sx0;
        sy = sy0;
        sz = sz0;
    }

    inline void scale(treal sx0, treal sy0, treal sz0)
    {
        mat4base scaleMatrix;
        scaleMatrix.sx = sx0;
        scaleMatrix.sy = sy0;
        scaleMatrix.sz = sz0;
        *this *= scaleMatrix;
    }

    inline void loadRotation(treal angle, treal ux, treal uy, treal uz)
    {
        angle = (treal)(angle * 3.14159265358979323846 / 180);

        treal inv_len = (treal)(1.0 / sqrt(ux * ux + uy * uy + uz * uz));
        ux *= inv_len;
        uy *= inv_len;
        uz *= inv_len;

        treal c = (treal)cos(angle);
        treal s = (treal)sin(angle);
        treal one_minus_c = (treal)1 - c;

        treal uxuy = ux * uy;
        treal uxuz = ux * uz;
        treal uyuz = uy * uz;

        treal sux = s * ux;
        treal suy = s * uy;
        treal suz = s * uz;

        a00 = c + one_minus_c * ux * ux;
        a10 = one_minus_c * uxuy + suz;
        a20 = one_minus_c * uxuz - suy;

        a01 = one_minus_c * uxuy - suz;
        a11 = c + one_minus_c * uy * uy;
        a21 = one_minus_c * uyuz + sux;

        a02 = one_minus_c * uxuz + suy;
        a12 = one_minus_c * uyuz - sux;
        a22 = c + one_minus_c * uz * uz;

        a30 = a31 = a32 = a03 = a13 = a23 = 0;
        a33 = 1;
    }

    inline void rotate(treal angle, treal ux, treal uy, treal uz)
    {
        mat4base rotationMatrix;
        rotationMatrix.loadRotation(angle, ux, uy, uz);
        *this *= rotationMatrix;
    }

    inline basevec4<treal> getRow(unsigned i) const
    {
        assert(i < 4);
        return
            basevec4<treal>(
            mat[0][i],
            mat[1][i],
            mat[2][i],
            mat[3][i]
        );
    }

    inline basevec4<treal> getColumn(unsigned i)const
    {
        assert(i < 4);
        return basevec4<treal>(mat[i]);
    }

    inline void setRow(unsigned i, const basevec4<treal>& v)
    {
        assert(i < 4);
        mat[0][i] = v.x;
        mat[1][i] = v.y;
        mat[2][i] = v.z;
        mat[3][i] = v.w;
    }

    inline void setColumn(unsigned i, const basevec4<treal> & v)
    {
        assert(i < 4);
        mat[i][0] = v.x;
        mat[i][1] = v.y;
        mat[i][2] = v.z;
        mat[i][3] = v.w;
    }

    inline void loadFrustum(treal left, treal right,
                            treal bottom, treal top,
                            treal zNear, treal zFar)
    {
        const treal n2 = zNear + zNear;
        const treal invRminusL = 1 / (right - left);
        const treal invFminusN = 1 / (zFar - zNear);
        const treal invTminusB = 1 / (top - bottom);

        a00 = n2 * invRminusL;	// 2*near/(right-left);
        a10 = a20 = a30 = a01 = 0;
        a11 = n2 * invTminusB;	// 2*near/(top-bottom)
        a21 = a31 = 0;
        a02 = (right + left) * invRminusL;	// (right+left)/(right-left);
        a12 = (top + bottom) * invTminusB;	// (top+bottom)/(top-bottom)
        a22 = -(zFar + zNear) * invFminusN;	// -(far+near)/(far-near)
        a32 = -1;
        a03 = a13 = 0;
        a23 = -n2 * zFar * invFminusN;		// -2*far*near/(far-near)
        a33 = 0;
    }

    inline void loadPerspective(double fovY, double aspect, double zNear, double zFar)
    {
        fovY = (treal)(fovY * 3.14159265358979323846 / 180);
        treal top = zNear * tan(fovY * 0.5);
        treal right = aspect * top;
        loadFrustum(-right, right, -top, top, zNear, zFar);
    }

    inline treal getDeterminant()const
    {
        mat3base<treal> m0(getMinorMatrix(0, 0));
        mat3base<treal> m1(getMinorMatrix(0, 1));
        mat3base<treal> m2(getMinorMatrix(0, 2));
        mat3base<treal> m3(getMinorMatrix(0, 3));
        return
            a00 * m0.getDeterminant() -
            a01 * m1.getDeterminant() +
            a02 * m1.getDeterminant() -
            a03 * m2.getDeterminant();
    }

    inline mat4base<treal> getInverseMatrix()const
    {
        const treal det = getDeterminant();
        const treal invDet = 1 / det;
        return mat4base<treal>(
            // column 0
            +getMinorMatrix(0, 0).getDeterminant() * invDet,
            -getMinorMatrix(0, 1).getDeterminant() * invDet,
            +getMinorMatrix(0, 2).getDeterminant() * invDet,
            -getMinorMatrix(0, 3).getDeterminant() * invDet,
            // column 1
            -getMinorMatrix(1, 0).getDeterminant() * invDet,
            +getMinorMatrix(1, 1).getDeterminant() * invDet,
            -getMinorMatrix(1, 2).getDeterminant() * invDet,
            +getMinorMatrix(1, 3).getDeterminant() * invDet,
            // column 2
            +getMinorMatrix(2, 0).getDeterminant() * invDet,
            -getMinorMatrix(2, 1).getDeterminant() * invDet,
            +getMinorMatrix(2, 2).getDeterminant() * invDet,
            -getMinorMatrix(2, 3).getDeterminant() * invDet,
            // column 3
            -getMinorMatrix(3, 0).getDeterminant() * invDet,
            +getMinorMatrix(3, 1).getDeterminant() * invDet,
            -getMinorMatrix(3, 2).getDeterminant() * invDet,
            +getMinorMatrix(3, 3).getDeterminant() * invDet
            );
    }

    inline mat3base<treal> getMinorMatrix(unsigned row, unsigned column)const
    {
        static const unsigned indices[4][3] =
        {
            {1, 2, 3},
            {0, 2, 3},
            {0, 1, 3},
            {0, 1, 2},
        };

        unsigned c0 = indices[column][0];
        unsigned c1 = indices[column][1];
        unsigned c2 = indices[column][2];
        unsigned r0 = indices[row][0];
        unsigned r1 = indices[row][1];
        unsigned r2 = indices[row][2];

        return mat3base<treal>(
            mat[c0][r0], mat[c0][r1], mat[c0][r2],
            mat[c1][r0], mat[c1][r1], mat[c1][r2],
            mat[c2][r0], mat[c2][r1], mat[c2][r2]
        );
    }

    inline void loadLookAtRH(treal eyeX, treal eyeY, treal eyeZ,
                             treal atX, treal atY, treal atZ,
                             treal upX, treal upY, treal upZ)
    {
        basevec3<treal> eye(eyeX, eyeY, eyeZ);
        basevec3<treal> at(atX, atY, atZ);
        basevec3<treal> up(upX, upY, upZ);

        basevec3<treal> zaxis = (eye - at);
        zaxis.normalize();
        basevec3<treal> xaxis = cross(up, zaxis);
        xaxis.normalize();
        basevec3<treal> yaxis = cross(zaxis, xaxis);
        yaxis.normalize();


        setRow(0, basevec4<treal>(xaxis));
        setRow(1, basevec4<treal>(yaxis));
        setRow(2, basevec4<treal>(zaxis));
        clearRow(3);
        x = -dot(xaxis, eye);
        y = -dot(yaxis, eye);
        z = -dot(zaxis, eye);
        w = 1;
    }

    inline void lookAtRH(treal eyeX, treal eyeY, treal eyeZ,
                         treal atX, treal atY, treal atZ,
                         treal upX, treal upY, treal upZ)
    {
        mat4base lookAtMatrix;
        lookAtMatrix.loadLookAtRH(eyeX, eyeY, eyeZ, atX, atY, atZ, upX, upY, upZ);
        *this *= lookAtMatrix;
    }

    inline void clearColumn(size_t i)
    {
        assert(i < 4);
        mat[i][0] = 0;
        mat[i][1] = 0;
        mat[i][2] = 0;
        mat[i][3] = 0;
    }

    inline void clearRow(size_t i)
    {
        assert(i < 4);
        mat[0][i] = 0;
        mat[1][i] = 0;
        mat[2][i] = 0;
        mat[3][i] = 0;
    }

    inline void loadIdentity()
    {
        static const treal identityMatrix[4][4] =
        {
            {1, 0, 0, 0},
            {0, 1, 0, 0},
            {0, 0, 1, 0},
            {0, 0, 0, 1},
        };

        memcpy(data, identityMatrix, sizeof(identityMatrix));
    }

    inline void normalize()
    {
        basevec3<treal> xAxis(data[0], data[4], data[8]);
        xAxis.Normalize();
        basevec3<treal> yAxis(data[1], data[5], data[9]);
        yAxis.Normalize();

        basevec3<treal> zAxis = cross(xAxis, yAxis);
        zAxis.Normalize();

        xAxis = cross(yAxis, zAxis);
        xAxis.Normalize();

        yAxis = cross(zAxis, xAxis);
        yAxis.Normalize();

        setRow(0, xAxis, x);
        setRow(1, yAxis, y);
        setRow(2, zAxis, z);
    }

    union
    {
        struct
        {
            union
            {
                treal a00;
                treal sx;
            };
            treal a10, a20;
            union
            {
                treal a30;
                treal p;
            };

            treal a01;
            union
            {
                treal a11;
                treal sy;
            };
            treal a21;
            union
            {
                treal a31;
                treal q;
            };

            treal a02;
            treal a12;
            union
            {
                treal a22;
                treal sz;
            };
            union
            {
                treal a32;
                treal r;
            };

            union
            {
                treal a03;
                treal x;
            };
            union
            {
                treal a13;
                treal y;
            };
            union
            {
                treal a23;
                treal z;
            };
            union
            {
                treal a33;
                treal w;
            };
        };

        treal mat[4][4];
        treal data[16];
    };
};

typedef mat4base<double> mat4d;
typedef mat4base<float> mat4f;

